package org.droidfly;

import java.io.IOException;
import java.util.AbstractCollection;
import java.util.LinkedList;

import org.droidfly.ServoControlService1.ServoControlServiceBinder;

import android.app.Service;
import android.content.ComponentName;
import android.content.Context;
import android.content.Intent;
import android.content.ServiceConnection;
import android.hardware.Sensor;
import android.hardware.SensorEvent;
import android.hardware.SensorEventListener;
import android.hardware.SensorManager;
import android.location.Location;
import android.location.LocationListener;
import android.location.LocationManager;
import android.os.Binder;
import android.os.Bundle;
import android.os.Handler;
import android.os.IBinder;
import android.os.Message;
import android.util.Log;

public class NavigationDataService extends Service {

	// Debugging
	private static final String TAG = "NavigationDataService";
	private static final boolean D = true;

	// Binder given to clients
	private final IBinder mBinder = new NavigationDataServiceBinder();
	private Handler mHandler;

	private LocationManager locationManager;
	private SensorManager sensorManager;

	private Sensor accelerometer;
	private Sensor magneticField;

	private float[] accelerometerValues = null;
	private float[] geomagneticMatrix = null;

	private final int maxValuesCount = 5;
	private LinkedList<Float> azimuthValues = new LinkedList<Float>();
	private LinkedList<Float> rollValues = new LinkedList<Float>();
	private LinkedList<Float> pitchValues = new LinkedList<Float>();

	private float azimuth = 0.0f;
	private float roll = 0.0f;
	private int pitch = 0;

	private Location currentLocation = new Location("dummy provider");
	private Location startLocation = new Location("dummy provider");
	private Location destinationLocation = new Location("dummy provider");

	public static final int MSG_LOCATION_CHANGED = 1;
	public static final int MSG_AZIMUTH_CHANGED = 2;
	public static final int MSG_PICH_CHANGED = 3;
	public static final int MSG_ROLL_CHANGED = 4;
	public static final int MSG_ORIENTATION_CHANGED = 5;

	/**
	 * Class used for the client Binder. Because we know this service always
	 * runs in the same process as its clients, we don't need to deal with IPC.
	 */
	public class NavigationDataServiceBinder extends Binder {
		NavigationDataService getService() {
			return NavigationDataService.this;
		}
	}

	@Override
	public IBinder onBind(Intent intent) {
		return mBinder;
	}

	@Override
	public void onCreate() {

		locationManager = (LocationManager) getSystemService(Context.LOCATION_SERVICE);
		sensorManager = (SensorManager) getSystemService(SENSOR_SERVICE);

		accelerometer = sensorManager
				.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
		magneticField = sensorManager
				.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD);

		sensorManager.registerListener(sensorEventListener, accelerometer,
				SensorManager.SENSOR_DELAY_UI);
		sensorManager.registerListener(sensorEventListener, magneticField,
				SensorManager.SENSOR_DELAY_UI);
		locationManager.requestLocationUpdates(LocationManager.GPS_PROVIDER, 0,
				0, locationListener);

		if (false == isServoControlServiceBound) {
			Intent intent = new Intent(this, ServoControlService1.class);
			bindService(intent, servoControlServiceConnection,
					Service.BIND_AUTO_CREATE);
		}
		
		if (D)
			Log.d(TAG, "NavigationDataService created");
	}
	
	
	@Override
	public void onDestroy() {
		sensorManager.unregisterListener(sensorEventListener);
		locationManager.removeUpdates(locationListener);
		if (isServoControlServiceBound) {
			unbindService(servoControlServiceConnection);
			isServoControlServiceBound = false;
		}
	}

	public void setHandler(Handler handler) {
		this.mHandler = handler;
	}

	public float getAzimuth() {
		return azimuth;
	}

	public int getPitch() {
		return pitch;
	}

	public float getRoll() {
		return roll;
	}

	public Location getCurrentLocation() {
		return currentLocation;
	}

	public void setStartLocation(Location location) {
		this.startLocation = location;
	}

	public Location getStartLocation() {
		return startLocation;
	}

	public void setDestinationLocation(Location location) {
		this.destinationLocation = location;
	}

	public Location getDestinationLocation() {
		return destinationLocation;
	}

	public float getDistanceToDestination() {
		return currentLocation.distanceTo(destinationLocation);
	}

	public float getDistanceToStart() {
		return currentLocation.distanceTo(startLocation);
	}

	public float getDistanceBetweenStartAndDestination() {
		return startLocation.distanceTo(destinationLocation);
	}

	public float getSpeed() {
		return currentLocation.getSpeed();
	}

	/**
	 * Direction from current to destination point
	 * 
	 * @return
	 */
	public float getBearing() {
		return currentLocation.bearingTo(destinationLocation);
	}

	/**
	 * Direction from start to destination point
	 * 
	 * @return
	 */
	public float getDesiredTrack() {
		return startLocation.bearingTo(destinationLocation);
	}

	/**
	 * Current direction
	 * 
	 * @return
	 */
	public float getTrack() {
		return currentLocation.getBearing();
	}

	// Define a listener that responds to location updates
	LocationListener locationListener = new LocationListener() {
		public void onLocationChanged(Location location) {
			performLocationChanged(location);
		}

		public void onStatusChanged(String provider, int status, Bundle extras) {
		}

		public void onProviderEnabled(String provider) {
		}

		public void onProviderDisabled(String provider) {
		}
	};

	SensorEventListener sensorEventListener = new SensorEventListener() {

		@Override
		public void onSensorChanged(SensorEvent event) {
			performOrientationChanged(event);

		}

		@Override
		public void onAccuracyChanged(Sensor sensor, int accuracy) {
			// TODO Auto-generated method stub
		}
	};

	// boolean sensorReady = true;

	private void performLocationChanged(Location location) {
		if (null == startLocation)
			startLocation = location;

		if (true == isBetterLocation(location, currentLocation)) {
			currentLocation = location;
			mHandler.obtainMessage(MSG_LOCATION_CHANGED, 0, 0).sendToTarget();
		}
	}

	/**
	 * Determines whether one Location reading is better than the current
	 * Location fix
	 * 
	 * @param location
	 *            The new Location that you want to evaluate
	 * @param currentBestLocation
	 *            The current Location fix, to which you want to compare the new
	 *            one
	 */
	private boolean isBetterLocation(Location location, Location currentLocation) {
		if (currentLocation == null) {
			// A new location is always better than no location
			return true;
		}

		int THREE_SECONDS = 1000 * 3;

		// Check whether the new location fix is newer or older
		long timeDelta = location.getTime() - currentLocation.getTime();
		boolean isSignificantlyNewer = timeDelta > THREE_SECONDS;

		// Check whether the new location fix is more or less accurate
		float accuracyDelta = location.getAccuracy()
				- currentLocation.getAccuracy();
		boolean isMoreAccurate = accuracyDelta < 0;
		double distance = location.distanceTo(currentLocation);
		boolean isSignificantlyLessAccurate = accuracyDelta > 10;

		// Determine location quality using a combination of timeliness and
		// accuracy

		if (isSignificantlyNewer) {
			return true;
		}
		if (isMoreAccurate) {
			return true;
		}
		if (isSignificantlyLessAccurate && accuracyDelta < distance / 2.0f) {
			return true;
		}
		return false;
	}

	private long lastUpdate = System.currentTimeMillis();

	int a, p, r;
	long aT, pT, rT;
	int pLastAvg = 0;

	LinkedList<Integer> pList = new LinkedList<Integer>();

	private void performOrientationChanged(SensorEvent event) {
		
		
		switch (event.sensor.getType()) {
		case Sensor.TYPE_ACCELEROMETER:
			accelerometerValues = event.values.clone();
			break;
		case Sensor.TYPE_MAGNETIC_FIELD:
			geomagneticMatrix = event.values.clone();
			// sensorReady = true;
			break;
		default:
			break;
		}

		if (geomagneticMatrix != null && accelerometerValues != null) {
			// sensorReady = false;
			float[] R = new float[16];
			float[] I = new float[16];

			if (true == SensorManager.getRotationMatrix(R, I,
					accelerometerValues, geomagneticMatrix)) {
				float[] actual_orientation = new float[3];
				SensorManager.getOrientation(R, actual_orientation);

				int azimuth = (int) (actual_orientation[0] * 180.0 / Math.PI);
				int pitch = (int) (actual_orientation[1] * 180.0 / Math.PI);
				int roll = (int) (actual_orientation[2] * 180.0 / Math.PI);

				long timeS = System.currentTimeMillis();
				long aTdelta = timeS - aT;
				long pTdelta = timeS - pT;
				long rTdelta = timeS - rT;

				int aDelta = Math.abs(a - azimuth);
				int pDelta = Math.abs(p - pitch);
				int rDelta = Math.abs(r - roll);

				if (pDelta < 1)
					return;

				aT = timeS;
				pT = timeS;
				rT = timeS;

				a = azimuth;
				p = pitch;
				r = roll;

				// sehr große abweichung in kurzer zeit
//				 if(pTdelta / pDelta < 50) 				
//					 return;

				pList.add(p);
				if (pList.size() > 3)
					pList.poll();

				int pAvg = 0;
				for (int pValue : pList) {
					pAvg += pValue;
				}

				pAvg = pAvg / pList.size();
				if (pAvg == pLastAvg)
					return;

				pLastAvg = pAvg;
				this.pitch = pLastAvg;
				
				if (isServoControlServiceBound) {
					try {
						servoControlService
								.moveServoToPosition(0, this.pitch +90 );
					} catch (IOException e) {

					}
				}
				
				if (isServoControlServiceBound) {
					try {
						servoControlService
						.moveServoToPosition(1, this.pitch +90 );
					} catch (IOException e) {

					}
				}
				
				mHandler.obtainMessage(MSG_PICH_CHANGED, p, pAvg)
				.sendToTarget();
			}
		}
	}

	// private void performOrientationChanged1(SensorEvent event) {
	//
	// // if (System.currentTimeMillis() - lastUpdate < 100)
	// // return;
	// //
	// // lastUpdate = System.currentTimeMillis();
	//
	//
	//
	// switch (event.sensor.getType()) {
	// case Sensor.TYPE_ACCELEROMETER:
	// accelerometerValues = event.values.clone();
	// break;
	// case Sensor.TYPE_MAGNETIC_FIELD:
	// geomagneticMatrix = event.values.clone();
	// // sensorReady = true;
	// break;
	// default:
	// break;
	// }
	//
	// if (geomagneticMatrix != null && accelerometerValues != null) {
	// // sensorReady = false;
	// float[] R = new float[16];
	// float[] I = new float[16];
	//
	// if (true == SensorManager.getRotationMatrix(R, I,
	// accelerometerValues, geomagneticMatrix)) {
	// float[] actual_orientation = new float[3];
	// SensorManager.getOrientation(R, actual_orientation);
	//
	// this.azimuth = actual_orientation[0];
	// mHandler.obtainMessage(MSG_AZIMUTH_CHANGED, 0, 0)
	// .sendToTarget();
	//
	// this.pitch = actual_orientation[1];
	// mHandler.obtainMessage(MSG_PICH_CHANGED, 0, 0)
	// .sendToTarget();
	//
	// this.roll = actual_orientation[2];
	// mHandler.obtainMessage(MSG_ROLL_CHANGED, 0, 0)
	// .sendToTarget();
	//
	// // addNewSensorValue(azimuthValues, actual_orientation[0]);
	// // addNewSensorValue(pitchValues, actual_orientation[1]);
	// // addNewSensorValue(rollValues, actual_orientation[2]);
	// //
	// // if (azimuthValues.size() < maxValuesCount
	// // || pitchValues.size() < maxValuesCount
	// // || rollValues.size() < maxValuesCount)
	// // return;
	//
	//
	//
	// // float azimuth = getAvg(azimuthValues);
	// // float pitch = getAvg(pitchValues);
	// // float roll = getAvg(rollValues);
	// //
	// // if (azimuth != this.azimuth) {
	// // this.azimuth = azimuth;
	// // mHandler.obtainMessage(MSG_AZIMUTH_CHANGED, 0, 0)
	// // .sendToTarget();
	// // }
	// //
	// // if (pitch != this.pitch) {
	// // this.pitch = pitch;
	// // mHandler.obtainMessage(MSG_PICH_CHANGED, 0, 0)
	// // .sendToTarget();
	// // }
	// //
	// // if (roll != this.roll) {
	// // this.roll = roll;
	// // mHandler.obtainMessage(MSG_ROLL_CHANGED, 0, 0)
	// // .sendToTarget();
	// // }
	// }
	// }
	// }

	private float getAvg(AbstractCollection<Float> values) {
		float sum = 0;
		for (Float value : values) {
			sum += value.floatValue();
		}
		return sum / values.size();

	}

	private float getMin(AbstractCollection<Float> values) {
		float min = Float.MAX_VALUE;
		for (Float value : values) {
			if (value < min)
				min = value;
		}
		return min;

	}

	private float getMax(AbstractCollection<Float> values) {
		float max = Float.MIN_VALUE;
		for (Float value : values) {
			if (value > max)
				max = value;
		}
		return max;

	}

	private void addNewSensorValue(LinkedList<Float> list, float value) {
		if (list.size() < 1) {
			list.add(value);
			return;
		}

		// float avgValue = getAvg(list);
		float minValue = getMin(list);
		float maxValue = getMax(list);

		if (value > minValue && value < maxValue)
			list.add(value);
		else if (value < minValue) {
			float difference = minValue - value;
			list.add(minValue - difference / 2.0f);
		} else {
			float difference = value - minValue;
			list.add(maxValue + difference / 2.0f);
		}

		if (list.size() > maxValuesCount)
			list.poll();
	}
	
	private final Handler handler = new Handler() {
		@Override
		public void handleMessage(Message msg) {
//			switch (msg.what) {
//			case NavigationDataService.MSG_AZIMUTH_CHANGED:		
//
//				break;
//			}
		}	
	};
	
	
	// / SERVICE	

	private boolean isServoControlServiceBound = false;
	private ServoControlService1 servoControlService = null;
	/** Defines callbacks for service binding, passed to bindService() */
	private ServiceConnection servoControlServiceConnection = new ServiceConnection() {

		@Override
		public void onServiceConnected(ComponentName className, IBinder service) {
			// We've bound to LocalService, cast the IBinder and get
			// LocalService instance
			ServoControlServiceBinder binder = (ServoControlServiceBinder) service;
			servoControlService = binder.getService();
			servoControlService.setHandler(handler);
			isServoControlServiceBound = true;
		}

		@Override
		public void onServiceDisconnected(ComponentName arg0) {
			isServoControlServiceBound = false;
		}
	};

}
